#! /usr/bin/env python3

import threading
from os import system, path, readlink
from struct import pack, unpack
from sys import exit
from time import time, sleep
from collections import deque
import serial
import re
import matplotlib.pyplot as plt

# ROS环境打开
from geometry_msgs.msg import Vector3Stamped
import rospy

HZ = 100

DOWN_FRAME_HEAD_1 = b'\xAA'
DOWN_FRAME_HEAD_2 = b'\xAA'
DOWN_FRAME_HEAD = DOWN_FRAME_HEAD_1 + DOWN_FRAME_HEAD_2

DOWN_FRAME_TAIL_1 = b'\x55'
DOWN_FRAME_TAIL_2 = b'\x55'
DOWN_FRAME_TAIL = DOWN_FRAME_TAIL_1 + DOWN_FRAME_TAIL_2

FRAME_LEN = 12

bAny, bS8, bU8, bS16, bU16, bS32, bU32, bF = 'x', 'b', 'B', 'h', 'H', 'i', 'I', 'f'
DOWN_PROTO = '<' + bU8 * FRAME_LEN

WAITING_DOWN_FRAME_HEAD_1 = 1
WAITING_DOWN_FRAME_HEAD_2 = 2
READING_DATA = 3
NUM_HEIGHT_SAMPLES = 50

from glob import glob

# Linux环境下串口读取
#PORT = glob('/dev/ttyUSB[0-9]*')[0]
#PORT = path.join('/dev', readlink(glob('/dev/serial/by-id/*usb-FTDI_FT232R_USB_UART_AB0L6OZ2-if00-port0*')[0]).split('/')[-1])
PORT = "/dev/ttyTHS1"
print("PORT: ", PORT)

## windows环境下串口读取
# PORT = ('COM3')
# print('PORT',PORT)

from signal import signal, SIGINT


def signal_handler(sig, frame):
    print('You pressed Ctrl+C!')
    exit(0)


signal(SIGINT, signal_handler)


def timer(tol=1):
    def decorator(func):
        def wrapper(*args, **kwargs):
            startTime = time()
            result = func(*args, **kwargs)
            endTime = time()
            # print(f'Time elap: {endTime - startTime:.2f}')
            if endTime - startTime < tol:
                sleep(tol - endTime + startTime)
            # print(f'Ended')
            return result

        return wrapper

    return decorator

# ROS环境打开
def is_hexadecimal(input_str):
    pattern = r"^[0-9A-Fa-f]{2}$"
    return bool(re.match(pattern, input_str))

def is_hexadecimal(byte):
    try:
        byte_value = int(byte, 16)
        return True
    except ValueError:
        return False 
    

# 初始化数据存储列表
time_values = []
height_values = []
height_samples = []

# 初始化图形
#plt.ion()
#fig, ax = plt.subplots()
# line, = ax.plot(time_values, height_values)

class HEIGHTSENSORCOMM:
    def __init__(self):
        self.init = False
        self.data_updated = False
        self.state = WAITING_DOWN_FRAME_HEAD_1

        self.height = -1.0
        self.heightDeque = deque(maxlen=50)
        self.heightAvg = -1.0


        self.downSer = serial.Serial(
            port=PORT,
            baudrate=115200,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            bytesize=serial.EIGHTBITS,
            timeout=0.5
        )
        self.dataBuf = bytearray()

        # ROS环境打开
        rospy.init_node('height_sensor_comm', anonymous=True)
        rospy.Rate(100) # 50Hz
        self.heightPub = rospy.Publisher('/skp40/height', Vector3Stamped, queue_size=1)

    def getTime_now(self):
        return time()
    
    def readData(self):
        # ROS环境打开，并注释下一行
        while not rospy.is_shutdown():
        # while True:
            data = self.downSer.read(1)
            if data:
                if self.state == WAITING_DOWN_FRAME_HEAD_1:
                    if data == DOWN_FRAME_HEAD_1:
                        self.state = WAITING_DOWN_FRAME_HEAD_2
                        # print('head_1',data)
                elif self.state == WAITING_DOWN_FRAME_HEAD_2:
                    if data == DOWN_FRAME_HEAD_2:
                        self.state = READING_DATA
                        dataBuf = bytearray()
                        # print('head_2',data)
                # elif data == 'U' or data == b'\n':
                #         pass
                elif self.state == READING_DATA :
                    
                        dataBuf.append(data[0])
                        # print('dataBuf',dataBuf)
                        if len(dataBuf) == FRAME_LEN:
                            print(DOWN_FRAME_HEAD.hex(),dataBuf.hex())
                            
                            downData = unpack(DOWN_PROTO, dataBuf)
                            
                            if (downData[-2] != 0x55):
                                pass
                            if (downData[-1] != 0x55):
                                pass
                            self.data_updated = True
                            
                            if (downData[0] == 12 ) & (downData[1] == 7):
                                print(DOWN_FRAME_HEAD.hex(),dataBuf.hex())
                                self.height = (downData[4] * 256.0 + downData[5]) * 0.01
                                self.heightDeque.append(self.height)
                                # print(downData[4])
                                # print(downData[5])
                                
                                if self.height == 0:
                                    self.height = -1.0

                                
                                # elif abs(self.height -self.heightDeque[-1]) > 3:
                                #     self.heightDeque[-1] = self.heightDeque[-2]
                                #     # self.heightreal = self.heightDeque[-1]
                                #     # print('currentstate', self.height)
                                #     # print('laststate', self.heightDeque[-1])
                                #     pass
                                    
                                # else:

                                #     print(f"Height is: %.2f m" % self.height)
                                #     height_samples.append(self.height)

                                #     if len(self.heightDeque) > 0:
                                #         self.heightAvg = sum(self.heightDeque) / NUM_HEIGHT_SAMPLES
                                #         height_values.append(self.heightAvg)
                                #         time_values.append(len(self.heightDeque))
                                #         # # 更新曲线
                                #         # line.set_xdata(time_values)
                                #         # line.set_ydata(height_values)
                                #         # ax.relim()
                                #         # ax.autoscale_view()
                                #         # # 刷新图形
                                #         # fig.canvas.draw()
                                #         # fig.canvas.flush_events()

                                #     # else:
                                #     #     self.heightAvg = self.height

                                print(f"Height average is: %.2f m" % self.heightAvg)

                            self.state = WAITING_DOWN_FRAME_HEAD_1

            if self.state == WAITING_DOWN_FRAME_HEAD_1:
                dataBuf = bytearray()

    def startRead(self):
        tRead = threading.Thread(target=self.readData)
        tRead.start()

    def printState(self):
        system('clear')
        # print('-' * 20)
        # print('Height: {self.height:.3f}m')
        # print(f'Deque: {self.heightDeque}')
        print("height: ",self.height )

    # ROS环境打开
    def rosPub(self):
        print(self.data_updated)
        if self.data_updated == True:
            msg = Vector3Stamped()
            msg.header.stamp = rospy.Time.now()
            msg.vector.x = self.height
            self.height = -1
            msg.vector.y = self.heightAvg
            self.heightPub.publish(msg)
            self.data_updated = False

    # ROS环境打开
    @timer(tol=1 / HZ)
    def spinOnce(self):
        self.printState()
        self.rosPub()

    # ROS环境打开
    def spin(self):
        self.startRead()
        while not rospy.is_shutdown():
        # while True:
            self.spinOnce()
    def __del__(self):
        print("destructor of the height snesor")
        self.downSer.close()



if __name__ == '__main__':
    # print(f'PORT is {PORT}')
    hsc = HEIGHTSENSORCOMM()
    hsc.spin()
    
